﻿using UnityEngine;
using System;
using System.Threading;
using System.Linq;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.IO;
using System.Text;
using IMIForUnity;

public partial class KDXBodyMeaImiDemo : IMIBodyMeasure
{
	//ImiWrapper.DEPTHWIDTH * ImiWrapper.DEPTHHEIGHT;

	//when we find a point on the boundary on a contour, such as body contour, we compare one pixel and the pixel next to it.
	//if both pixel are inside the body contour, we assume that the depth difference betweent them is smaller than  boundThreshval.
	//if not, we take the first spot as the one one the boundary of the contour.
	public  ushort boundThreshval = 300;   //unit mm  0.3m

	//Set the tilting down angle. it is in the range from14 to 
	float setDeviceHeight = 1700;  // In Default, we put the imi device on the height of 170 cm.
	float tiltDownAngle = 17;
	float HeightDiff = 0f;
	float halfvfov = 24f;

	//just for showing pixel poses.
	int headOrigPixPos = 0;
	int headTopPixPosY = 0;
	int headOrigDepth = 0;
	int headTopDepth = 0;
	public Dictionary<uint, float> PlayHeightDic;
	public FloatSmoothTool heightSmoothTool;
	float playerHeight = 0f;

	//pix pos for some parts of the body
	int hipsPixPosX = 0;
	int hipsPixPosY = 0;
	int hipsPixPosZ = 0;
	public KDXIMILineAnalysis mWaistLineAnalysis;
	public ClusterSmoothTool waistSmthWideness;
	float waistThicknessRatio = 0.94f;
	float waistWidenessRatio = 0.94f;
	public float waistWideness = 0;  //We set the distance between shoulders *chestWidShoulderWidthRatio as the wideness of chest.
	float waistWidShoulderWidthRatio = 0.96f;
	float waistRatio = 0.89f;  //the value detected by knt is a bit too large. 

	bool enableFrontProcess=false;

	void AddColorToDepthTex ()
	{
		int w = ImiWrapper.DEPTHWIDTH;
		int h = ImiWrapper.DEPTHHEIGHT;

		for (int i=0; i<100; i++) {
			for (int j=0; j<100; j++) {
				usersDepthColors [(j * w + i) * 4] = (byte)(255);
				usersDepthColors [(j * w + i) * 4 + 1] = (byte)(0);
				usersDepthColors [(j * w + i) * 4 + 2] = (byte)(0);
				usersDepthColors [(j * w + i) * 4 + 3] = 10;
			}
		}
	}


	void PrecessFront( uint userID, bool isDrawOnDepthMap  )
	{
		GetHeightForPlayer (userID);
		ProcessHipsAndWaist (userID, isDrawOnDepthMap);
	}



	public void GetHeightForPlayer (uint userID)
	{
		float currHeight = GetHeight (userID);
		heightSmoothTool.InsertValue (currHeight);
		playerHeight = heightSmoothTool.GetFilteredValue ();
	}

	public void ProcessHipsAndWaist (uint userID, bool isDrawOnDepthMap)
	{
		GetPlayerJointPixPos (userID, ref hipsPixPosX, ref hipsPixPosY, ref hipsPixPosZ, 
		                     ImiWrapper.ImiSkeletonPositionIndex.IMI_SKELETON_POSITION_HIP_CENTER, isDrawOnDepthMap);
		int w = ImiWrapper.DEPTHWIDTH;
		int h = ImiWrapper.DEPTHHEIGHT;

		//int waistDepth=(ushort)depthPlayerData[hipsPixPosY*w+ hipsPixPosX]>>3;
		GetWaistFrontWidth (depthPlayerData, hipsPixPosX, hipsPixPosY, boundThreshval, true);
	}
	
	public void GetPlayerJointPixPos (uint userID, ref int PosX, ref int PosY, ref int PosZ, 
	                                 ImiWrapper.ImiSkeletonPositionIndex jointIDX, bool isDrawOnDepthMap)
	{
		int w = ImiWrapper.DEPTHWIDTH;
		int h = ImiWrapper.DEPTHHEIGHT;
		
		
		
		ImiWrapper.ImiVector4 jointPosV4 =
			playerInfos [userID].
				jointsPosV4 [(int)jointIDX];
		
		ImiWrapper.ConvertSkeletonPointToDepthPoint (
			jointPosV4,
			ref PosX,
			ref PosY,
			ref PosZ,
			ImiWrapper.ImiImageResolution.IMI_IMAGE_RESOLUTION_640x480);
		
		
		if (isDrawOnDepthMap)
		if (isSpotInsideDepthMap (hipsPixPosX, hipsPixPosY)) {
			DrawingSpotsOnTexture.DrawPixelsSquareOnByteArray (w, h, hipsPixPosX, hipsPixPosY, 7, usersDepthColors, 0, 255, 0, 255);		
		}	


	}

	private void DrawJointsMarksOnDepthMap (uint controlId, ImiWrapper.ImiSkeletonPositionIndex  skeletonPosIdx, int pixelWidth, byte r, byte g, byte b, byte a)
	{
		int depthX = 0;
		int depthY = 0;
		int depthZ = 0;
	
		int w = ImiWrapper.DEPTHWIDTH;
		int h = ImiWrapper.DEPTHHEIGHT;



		ImiWrapper.ImiVector4 jointPosV4 =
				playerInfos [controlId].
				jointsPosV4 [(int)skeletonPosIdx];
		
		ImiWrapper.ConvertSkeletonPointToDepthPoint (
			jointPosV4,
			ref depthX,
			ref depthY,
			ref depthZ,
			ImiWrapper.ImiImageResolution.IMI_IMAGE_RESOLUTION_640x480);



		if (isSpotInsideDepthMap (depthX, depthY)) {
			DrawingSpotsOnTexture.DrawPixelsSquareOnByteArray (w, h, depthX, depthY, pixelWidth, usersDepthColors, 0, 255, 0, 255);		
		}	
	}

	float GetHeight (uint controlId)
	{
		int headDepthX = 0;
		int headDepthY = 0;
		int depthZ = 0;
		
		int w = ImiWrapper.DEPTHWIDTH;
		int h = ImiWrapper.DEPTHHEIGHT;
		
		
		
		ImiWrapper.ImiVector4 jointPosV4 =
			playerInfos [controlId].
				jointsPosV4 [(int)ImiWrapper.ImiSkeletonPositionIndex.IMI_SKELETON_POSITION_HEAD];
		
		ImiWrapper.ConvertSkeletonPointToDepthPoint (
			jointPosV4,
			ref headDepthX,
			ref headDepthY,
			ref depthZ,
			ImiWrapper.ImiImageResolution.IMI_IMAGE_RESOLUTION_640x480);


		//Get the pixel pos of head and head top.
		int pixDist_HeadTop_And_HeadOrig = IMIBodyMeaTool.GetHead2TopPixDist (depthPlayerData, w, headDepthX, headDepthY, boundThreshval);
		int topY = headDepthY - pixDist_HeadTop_And_HeadOrig;
				

		float bodyHeight = 0f;
		//Draw head spots and the head top spot.
		if (isSpotInsideDepthMap (headDepthX, headDepthY)) {
			headOrigPixPos = headDepthY;
			headTopPixPosY = topY;
			headOrigDepth = depthPlayerData [headDepthY * w + headDepthX] >> 3;
			headTopDepth = depthPlayerData [topY * w + headDepthX] >> 3; 

			DrawingSpotsOnTexture.DrawPixelsSquareOnByteArray (w, h, headDepthX, headDepthY, 3, usersDepthColors, 0, 255, 0, 255);

			//getSomeDepthValue(depthPlayerData, w,headDepthX,headDepthY,boundThreshval);
			//DrawingSpotsOnTexture.DrawPixelsSquareOnByteArray(w,h,headDepthX,topY,5,usersDepthColors,255,255,0,255);	


			//calculate the player height  according to the pixel pos of headTop and VerticalPlanePixPosY and headTopDepth.
			int dw = ImiWrapper.DEPTHWIDTH;
			int dh = ImiWrapper.DEPTHHEIGHT;

			bodyHeight = IMIBodyMeaTool.GetHeightByHeadTopPixPos (dh, dw, tiltDownAngle, 48, headTopPixPosY, headTopDepth, setDeviceHeight);

		}	

		return bodyHeight;
	}

	bool isSpotInsideDepthMap (int depthX, int depthY)
	{
		if ((depthX >= 0)
			&& (depthX < ImiWrapper.DEPTHWIDTH)
			&& (depthY >= 0)
			&& (depthY < ImiWrapper.DEPTHHEIGHT)) {
			return true;		
		}	

		return false;
	}



	//In IMI for now we just use hisps pixpos as the waist pixpos. because it seems the hips pos of IMI is a bit higher than usually
	void GetWaistFrontWidth (ushort[] _depthPlayerData, int waistPixPosX, int waistPixPosY, ushort boundThreshval, bool isDrawOrNot)
	{
		int dw = ImiWrapper.DEPTHWIDTH;
		int dh = ImiWrapper.DEPTHHEIGHT;
		Vector2 MinWaistPixPos = IMIBodyMeaTool.GetContourMinEnd (dw, dh, waistPixPosX, waistPixPosY, _depthPlayerData, boundThreshval);

		Vector2 MaxWaistPixPos = IMIBodyMeaTool.GetContourMaxEnd (dw, dh, waistPixPosX, waistPixPosY, _depthPlayerData, boundThreshval);




//		Vector2 resultWaistEnds = mWaistLineAnalysis.GetAdjustetdWaistEnds( _depthPlayerData,
//		                                                                    (int)MinWaistPixPos.x,
//		                                                                    (int)MaxWaistPixPos.x,
//		                                                                    (int)MinWaistPixPos.y);



	
		Vector2 resultWaistEndMin = Vector2.zero;  //this is the pix pos of 2 ends of waist
		Vector2 resultWaistEndMax = Vector2.zero;  //this is the pix pos of 2 ends of waist

		IMIBodyMeaTool.GetFinalWaistEnds(MinWaistPixPos,MaxWaistPixPos,ref resultWaistEndMin,ref resultWaistEndMax);

	

		if (isDrawOrNot) {
			DrawHorilineOnUserDepthColor (usersDepthColors, dw, dh, (int)MinWaistPixPos.x, (int)MaxWaistPixPos.x, (int)MaxWaistPixPos.y, 255, 255, 0, 255);
			DrawHorilineOnUserDepthColor (usersDepthColors, dw, dh, (int)resultWaistEndMin.x, (int)resultWaistEndMax.x, (int)resultWaistEndMax.y, 255, 0, 0, 255);
		}

		int WaistDepthMinEnd = ((_depthPlayerData [((int)(resultWaistEndMin.y)) * dw + ((int)(resultWaistEndMin.x))]) >> 3);
		int WaistDepthMaxEnd = ((_depthPlayerData [((int)(resultWaistEndMax.y)) * dw + ((int)(resultWaistEndMax.x))]) >> 3);
		int waistDepth = (WaistDepthMinEnd + WaistDepthMaxEnd) / 2;

		float tmpWid = GetThicknessBy2PixPos (resultWaistEndMin, resultWaistEndMax, waistDepth);

		if (tmpWid > 0) {
			waistSmthWideness.InsertValue (tmpWid);
			waistWideness = waistSmthWideness.GetFilteredValue () * waistWidenessRatio;	
		}	
	}

	void DrawHorilineOnUserDepthColor (byte[]  _usersDepthColors, int depthW, int depthH, int pixposXMin, int pixposXMax, int pixposY, byte r, byte g, byte b, byte a)
	{
		for (int i=pixposXMin; i<pixposXMax; i++) {
			usersDepthColors [(pixposY * depthW + i) * 4] = (byte)(r);
			usersDepthColors [(pixposY * depthW + i) * 4 + 1] = (byte)(g);
			usersDepthColors [(pixposY * depthW + i) * 4 + 2] = (byte)(b);
			usersDepthColors [(pixposY * depthW + i) * 4 + 3] = (byte)a;
		}
	}

	float GetThicknessBy2PixPos (Vector2 pixPos0, Vector2 pixPos1, int depthVal)
	{
		Vector3 WorldPosEnd0 = GetWorldPos_by_DepthPixCoord2 ((int)(pixPos0.x), (int)(pixPos0.y), depthVal);
		Vector3 WorldPosEnd1 = GetWorldPos_by_DepthPixCoord2 ((int)(pixPos1.x), (int)(pixPos1.y), depthVal);
		return Mathf.Abs (WorldPosEnd0.x - WorldPosEnd1.x);
	}
	
//
	public Vector3 GetWorldPos_by_DepthPixCoord2 (int PixPosX, int PixPosY, int depthval)
	{
		ImiWrapper.ImiVector4 _jointPosV4 = new ImiWrapper.ImiVector4 ();
		ImiWrapper.ConvertDepthPointToSkeletonPoint (
			PixPosX,
			PixPosY,
			depthval,
			ref _jointPosV4,
			ImiWrapper.ImiImageResolution.IMI_IMAGE_RESOLUTION_640x480);

		return  new Vector3 (_jointPosV4.x, _jointPosV4.y, _jointPosV4.z);
	}

	void OnGUI ()
	{
//		GUILayout.Label("origHeadDepth:   "+depthheadOrig.ToString());
//		GUILayout.Label("HeadDepth1:   "+depthhead1.ToString());
//		GUILayout.Label("HeadDepth2:   "+depthhead2.ToString());
//		GUILayout.Label("HeadDepth3:   "+depthhead3.ToString());
//		GUILayout.Label("HeadDepth4:   "+depthhead4.ToString());
//		GUILayout.Label("HeadDepth5:   "+depthhead5.ToString());

//		GUILayout.Label("HeadOrigPixPos:   "+headOrigPixPos.ToString());
//		GUILayout.Label("headTopPixPosY:   "+headTopPixPosY.ToString());
//		GUILayout.Label("HeadOrigDepth:   "+headOrigDepth.ToString());


		switch(mbodymeaState)
		{
		case BODYMEASTATE.IDLE0:
			GUILayout.Label ("IN Idle State 0");
			break;
		case BODYMEASTATE.FRONT:
			GUILayout.Label ("During Measurement State");		
			GUILayout.Label ("Player Height In Measure :   " + playerHeight.ToString ());		
			GUILayout.Label ("Player Waist Width :   " + waistWideness.ToString ());
			break;
		case BODYMEASTATE.IDLE1:
			GUILayout.Label ("Player Height :   " + playerHeight.ToString ());	
			break;
		default:
			break;
		}


		
//		if(PlayHeightDic.Count>0)
//		{
//			foreach (KeyValuePair<uint, float> pair in PlayHeightDic)
//			{
//				GUILayout.Label( pair.Key.ToString()+"   PlayerHeight:   "+pair.Value.ToString());
//			}
//		}
	}





	//code for getting depth values of some points beside the point of head
	//	int depthheadOrig=0;
	//	int depthhead1=0;
	//	int depthhead2=0;
	//	int depthhead3=0;
	//	int depthhead4=0;
	//	int depthhead5=0;
	//
	//
	//
	//	private void getSomeDepthValue (ushort[] depthPlayerData, int depthW, int OrigHeaddepthMapPixX, int OrigHeaddepthMapPixY, int boundThreshval)
	//	{
	//		// if the depth val of high point is larger than the head point by this value,  we  regard that point as a backpoint.  
	//		//ushort threshval = 60; //mm
	//		int dist = 0;
	//		int headX = OrigHeaddepthMapPixX;
	//		int headY = OrigHeaddepthMapPixY;
	//		int headOrigIdx = headY * depthW + headX;
	//		//the idx which is higher than the head;
	//		int headHighIdx = headOrigIdx;
	//		//Compare the point higher than the certain point/head point, one pix by one pix. Until we found the one on the border of the contour.
	//
	//		depthheadOrig=depthPlayerData [headHighIdx]>>3;
	//		depthhead1=depthPlayerData [headHighIdx-depthW]>>3;
	//		depthhead2=depthPlayerData [headHighIdx-depthW*2]>>3;
	//		depthhead3=depthPlayerData [headHighIdx-depthW*3]>>3;
	//		depthhead4=depthPlayerData [headHighIdx-depthW*4]>>3;
	//		depthhead5=depthPlayerData [headHighIdx-depthW*5]>>3;
	//	}

	
}
